#ifndef _COST_H_
#define _COST_H_

#include "track/object_track.h"
#include <algorithm>
#include <array>
#include <boost/foreach.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <memory>
#include <vector>

namespace bg = boost::geometry;
using point_t = bg::model::d2::point_xy<float>;
using polygon_t = bg::model::polygon<point_t>;
namespace autodrive {
namespace perception {
class Cost final {
 public:
  Cost() noexcept;
  Cost(const Cost& temp) = delete;
  Cost& operator=(const Cost& temp) = delete;
  Cost(Cost&& temp) = delete;
  Cost& operator=(Cost&& temp) = delete;

  /// @brief compute location distance
  static float ComputerLocationDistance(
      ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
      TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute direction distance
  static float ComputerDirectionDistance(
      ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
      TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute bbox size distance
  static float ComputerBBoxSizeDistance(
      ObjectTrackPtr const& track, TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute bbox size distance
  static float ComputerCartesianDistance2D(
      ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
      TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute track&new object Iou2d
  static float ComputerIOU2D(ObjectTrackPtr const& track,
                             Eigen::VectorXf const& track_predict,
                             TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute track&new object Iou2d
  static float ComputerIOU2DWithPos(ObjectTrackPtr const& track,
                                    Eigen::VectorXf const& track_predict,
                                    TrackedObjectPtr const& new_object,
                                    const Eigen::Matrix4f& pos) noexcept;

  /// @brief compute track&new object Iou3d
  static float ComputerIOU3D(ObjectTrackPtr const& track,
                             Eigen::VectorXf const& track_predict,
                             TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute track&new object gIou2d
  float ComputerGIOU2D(ObjectTrackPtr const& track,
                       Eigen::VectorXf const& track_predict,
                       TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute track&new object Iou3d
  static float ComputerGIOU3D(ObjectTrackPtr const& track,
                              Eigen::VectorXf const& track_predict,
                              TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute track&new Cartesian 2d distance
  static float ComputerCartesian2DDistance(
      ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
      TrackedObjectPtr const& new_object) noexcept;

  /// @brief compute track&new Cartesian 2d distance
  static float ComputerCartesian2DDistanceWithPos(
      ObjectTrackPtr const& track, Eigen::VectorXf const& track_predict,
      TrackedObjectPtr const& new_object, const Eigen::Matrix4f& pos) noexcept;

 private:
  /// @brief calculate iou 2d with two boxes, intersection/union
  /// @param b1 box1
  /// @param b2 box2
  /// @return float  iou 2d value
  static float iou2d(const Box& b1, const Box& b2);

  /// @brief calculate iou 3d with two boxes, intersection/union
  /// @param b1 box1
  /// @param b2 box2
  /// @return float  iou 3d value
  static float iou3d(const Box& b1, const Box& b2);

  /// @brief calculate giou 2d with two boxes,
  /// intersection/union-(convex hull-union)/(convex hull)
  /// @param b1 box1
  /// @param b2 box2
  /// @return float  iou 2d value
  static float giou2d(const Box& b1, const Box& b2);

  /// @brief calculate giou 3d with two boxes,
  /// intersection/union-(convex hull-union)/(convex hull)
  /// @param b1 box1
  /// @param b2 box2
  /// @return float  iou 3d value
  static float giou3d(const Box& b1, const Box& b2);

  ///@brief convert Object to Box
  static void object_to_box(ObjectTrackPtr const& track, Box& b) noexcept;
  static void object_to_box(TrackedObjectPtr const& track, Box& b) noexcept;
  static void object_to_box(ObjectTrackPtr const& track, Box& b,
                            const Eigen::Matrix4f& pos) noexcept;
  static void object_to_box(TrackedObjectPtr const& track, Box& b,
                            const Eigen::Matrix4f& pos) noexcept;
};

}  // namespace perception
}  // namespace autodrive

#endif
